<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package frc3128.Util;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import frc3128.HardwareLink.Motor.MotorDir;<a name="line.3"></a>
<span class="sourceLineNo">004</span><a name="line.4"></a>
<span class="sourceLineNo">005</span>/**<a name="line.5"></a>
<span class="sourceLineNo">006</span> *<a name="line.6"></a>
<span class="sourceLineNo">007</span> * @author Noah Sutton-Smolin<a name="line.7"></a>
<span class="sourceLineNo">008</span> */<a name="line.8"></a>
<span class="sourceLineNo">009</span>public class RobotMath {<a name="line.9"></a>
<span class="sourceLineNo">010</span>    /**<a name="line.10"></a>
<span class="sourceLineNo">011</span>     * Limits the angle to between 0 and 359 degrees for all math. All angles<a name="line.11"></a>
<span class="sourceLineNo">012</span>     * should be normalized before use.<a name="line.12"></a>
<span class="sourceLineNo">013</span>     * <a name="line.13"></a>
<span class="sourceLineNo">014</span>     * @param angle the angle to be normalized<a name="line.14"></a>
<span class="sourceLineNo">015</span>     * @return the normalized angle on [0,359]<a name="line.15"></a>
<span class="sourceLineNo">016</span>     */<a name="line.16"></a>
<span class="sourceLineNo">017</span>    public static double normalizeAngle(double angle) {<a name="line.17"></a>
<span class="sourceLineNo">018</span>        return ((angle % 360) + 360) % 360;<a name="line.18"></a>
<span class="sourceLineNo">019</span>    }<a name="line.19"></a>
<span class="sourceLineNo">020</span>    <a name="line.20"></a>
<span class="sourceLineNo">021</span>    /**<a name="line.21"></a>
<span class="sourceLineNo">022</span>     * <a name="line.22"></a>
<span class="sourceLineNo">023</span>     * @param pow motor power<a name="line.23"></a>
<span class="sourceLineNo">024</span>     * @return whether or not the power is valid<a name="line.24"></a>
<span class="sourceLineNo">025</span>     */<a name="line.25"></a>
<span class="sourceLineNo">026</span>    public static boolean isValidPower(double pow) {return (pow &gt;= -1 &amp;&amp; pow &lt;= 0);}<a name="line.26"></a>
<span class="sourceLineNo">027</span>    <a name="line.27"></a>
<span class="sourceLineNo">028</span>    /**<a name="line.28"></a>
<span class="sourceLineNo">029</span>     * Makes the provided power into a valid power level.<a name="line.29"></a>
<span class="sourceLineNo">030</span>     * <a name="line.30"></a>
<span class="sourceLineNo">031</span>     * @param pow power level to convert<a name="line.31"></a>
<span class="sourceLineNo">032</span>     * @return a properly-limited power level<a name="line.32"></a>
<span class="sourceLineNo">033</span>     */<a name="line.33"></a>
<span class="sourceLineNo">034</span>    public static double makeValidPower(double pow) {return (pow &lt; -1 ? -1 : (pow &gt; 1 ? 1 : pow));}<a name="line.34"></a>
<span class="sourceLineNo">035</span>    <a name="line.35"></a>
<span class="sourceLineNo">036</span>    /**<a name="line.36"></a>
<span class="sourceLineNo">037</span>     * Determines the appropriate direction for a motor to turn.<a name="line.37"></a>
<span class="sourceLineNo">038</span>     * <a name="line.38"></a>
<span class="sourceLineNo">039</span>     * @param currentAngle the current angle of the motor<a name="line.39"></a>
<span class="sourceLineNo">040</span>     * @param targetAngle the target angle of the motor<a name="line.40"></a>
<span class="sourceLineNo">041</span>     * @return an integer value on {-1,0,1} in MotorDir<a name="line.41"></a>
<span class="sourceLineNo">042</span>     */<a name="line.42"></a>
<span class="sourceLineNo">043</span>    public static int getMotorDirToTarget(double currentAngle, double targetAngle) {<a name="line.43"></a>
<span class="sourceLineNo">044</span>        currentAngle = RobotMath.normalizeAngle(currentAngle);<a name="line.44"></a>
<span class="sourceLineNo">045</span>        targetAngle = RobotMath.normalizeAngle(targetAngle);<a name="line.45"></a>
<span class="sourceLineNo">046</span>        int retDir = 1 * (Math.abs(currentAngle - targetAngle) &gt; 180 ? 1 : -1) * (currentAngle - targetAngle &lt; 0 ? -1 : 1);<a name="line.46"></a>
<span class="sourceLineNo">047</span>        <a name="line.47"></a>
<span class="sourceLineNo">048</span>        //MotorDir is syntactic sugar; could just return retDir<a name="line.48"></a>
<span class="sourceLineNo">049</span>        if(currentAngle - targetAngle == 0 || currentAngle - targetAngle == 180) return MotorDir.NONE;<a name="line.49"></a>
<span class="sourceLineNo">050</span>        return (retDir == 1 ? MotorDir.CW : MotorDir.CCW); <a name="line.50"></a>
<span class="sourceLineNo">051</span>    }<a name="line.51"></a>
<span class="sourceLineNo">052</span>    <a name="line.52"></a>
<span class="sourceLineNo">053</span>    private RobotMath() {}<a name="line.53"></a>
<span class="sourceLineNo">054</span>}<a name="line.54"></a>




























































</pre>
</div>
</body>
</html>
